#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/Quaternion.h>

#include <iostream>
#include <sstream>
using namespace std;

class ImuTest{
    public:
        ImuTest();
        ~ImuTest();

        void ImuCallback(const sensor_msgs::ImuConstPtr& msg);

    private:
        ros::NodeHandle __nh;
        ros::Subscriber __imu_sub;
        ros::Publisher __path_pub;

        boost::shared_ptr<const sensor_msgs::Imu> __imu_data_ptr;

        geometry_msgs::Twist __robot_vel;
        geometry_msgs::PoseStamped __robot_pose_stamp;
        float __robot_yaw = 0.0;
        vector<geometry_msgs::PoseStamped> __robot_poses;

        float ax_bias = 0, ay_bias = 0, az_bias = 0;
        float gx_bias = 0, gy_bias = 0, gz_bias = 0;
        int num_samples = 0;  
};


ImuTest::ImuTest():__imu_data_ptr(nullptr){
    __imu_sub = __nh.subscribe("/imu", 10, &ImuTest::ImuCallback, this);
    __path_pub = __nh.advertise<nav_msgs::Path>("/imu2path", 1);
}


ImuTest::~ImuTest(){

}


void ImuTest::ImuCallback(const sensor_msgs::ImuConstPtr& msg){
    if(!__imu_data_ptr){
        __imu_data_ptr = msg;
        return;
    }

    // 获取加速度和角速度数据
    float ax = msg->linear_acceleration.x;
    float ay = msg->linear_acceleration.y;
    float az = msg->linear_acceleration.z;
    float wx = msg->angular_velocity.x;
    float wy = msg->angular_velocity.y;
    float wz = msg->angular_velocity.z;

    // 计算零偏
    num_samples ++;
    if(num_samples <= 50){       //此处静止1秒计算零偏
        ax_bias += ax;
        ay_bias += ay;
        az_bias += az;
        gx_bias += wx;
        gy_bias += wy;
        gz_bias += wz;        
    }
    else if(num_samples == 51){
        ax_bias /= num_samples;
        ay_bias /= num_samples;
        az_bias /= num_samples;
        gx_bias /= num_samples;
        gy_bias /= num_samples;
        gz_bias /= num_samples;
        ROS_INFO("ax_bias=%.3f, ay_bias=%.3f, gz_bias=%.3f", ax_bias, ay_bias, gz_bias);
    }
    else{
        double dt = (msg->header.stamp - __imu_data_ptr->header.stamp).toSec();

        float vx_dt = (ax - ax_bias) * dt;
        float vy_dt = (ay - ay_bias) * dt;
        float gz_dt = (wz - gz_bias) * dt;
        vx_dt = fabs(vx_dt) < 0.02 ? 0.0 : vx_dt;
        vy_dt = fabs(vy_dt) < 0.02 ? 0.0 : vy_dt;
        gz_dt = fabs(gz_dt) < 0.02 ? 0.0 : gz_dt;

        __robot_vel.linear.x += vx_dt;
        __robot_vel.linear.y += vy_dt;
        __robot_vel.angular.z += gz_dt;

        // 计算位姿
        __robot_pose_stamp.pose.position.x += __robot_vel.linear.x * dt;
        __robot_pose_stamp.pose.position.y += __robot_vel.linear.y * dt;
        __robot_yaw = __robot_vel.angular.z * dt;
        tf2::Quaternion quaternion;
        quaternion.setRPY(0.0, 0.0, __robot_yaw);
        __robot_pose_stamp.pose.orientation = tf2::toMsg(quaternion);
        ROS_INFO("vel: [vx=%.3f, vy=%.3f, yaw=%.3f], robot_pose: [x=%.3f, y=%.3f, yaw=%.3f]", 
            __robot_vel.linear.x,
            __robot_vel.linear.y,
            __robot_vel.angular.z,
            __robot_pose_stamp.pose.position.x, 
            __robot_pose_stamp.pose.position.x, 
            __robot_yaw);

        // 发布path数据
        nav_msgs::Path nav_msgs;
        nav_msgs.header.frame_id = "odom";
        // nav_msgs.header.stamp = msg->header.stamp;
        nav_msgs.header.stamp = ros::Time::now();
        __robot_poses.emplace_back(__robot_pose_stamp);
        if(__robot_poses.size() > 5000){
            __robot_poses.erase(__robot_poses.begin());
        }
        nav_msgs.poses = __robot_poses;
        __path_pub.publish(nav_msgs);
    }





}


/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "test_imu2path");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>

    ROS_INFO("test_imu2path initializating......");
    ros::NodeHandle nh;
    ImuTest imu_test;
    ros::spin();
    return 0;
}